#pragma once
#ifndef _TRAJECTORY_H
#define _TRAJECTORY_H

#include <string>
#include <map>
#include <sophus/se3.hpp>
#include <sophus/sim3.hpp>
#include <sophus/rxso3.hpp>

using namespace std;


struct Config {
  //different methods, when FRANK is true, others are invalid
  bool IS_USE_FRANK = false;
  bool IS_USE_SALAS = false;
  bool IS_USE_UMEYAMA = false;
  bool IS_CONSIDER_ROT = false;
  float rot_weight = 0.8;

  //True: the attitude in pose is [R^T, t]: Pw = R^T*Pc + t
  //False: [R, t]: Pw = R*Pc + t
  bool IS_ROT_TRANSPOSE = false;

  bool IS_PLOT_MATCHLINE = false;
  bool IS_ALIGN = false;
  bool IS_ALIGN_SCALE = false;
  bool IS_DISPLAY = false;
  bool IS_OUTPUT = false;

  double t_offset = 0;
  string output_dir = "output/";
  //This was used to reverse the sign of euler angles, in case of different pose definition
  vector<int> rs;
  //Ground truth timestamp interval
  //float dt = 0.04;

  //used for determine the Threshold for euler Singular Value Filtering
  // g is for groundtruth and e is for odometry
  //The faster the odometer rotates, the r_sh needs to be set higher
  float g_r_sh = 0.8;
  float e_r_sh = 0.8;

  enum ROT_ORDER{
    ORDER_XYZ = 0,
    ORDER_ZXY = 1,
  } rot_order = ORDER_ZXY;
  //} rot_order = ORDER_XYZ;
 
  //set initial Euler angles to 0
  //bool IS_INIT_ANGLE_0 = false;
  bool IS_INIT_ANGLE_0 = true;
};

class Trajectory {
public:
  Trajectory() {}
  explicit Trajectory(const string &file_name);
  Trajectory(const string &file_name, const Config &c);

  void display_locs();
  //parse out output name from file_name
  string output_speci_name_get();

  string file_name;

  //save the origin stamp and poses of Trajectory
  map<int, pair<double, Sophus::SE3d>> stamp_poses;

  // index of euler reverse vector : rs in Config
  static int id_rs;
protected:
  int read_from_file(const string &file_name);
  void data_output();
  void gnuplot_script_generate();
  void matlab_script_generate();

  Config conf;

  //the color of match line rand seed
  static int random_seed;

  int euler_reverse = 1;
};
int Trajectory::id_rs = 0;
int Trajectory::random_seed = 1;

class Gt_traj: public Trajectory {
public:
  Gt_traj() {}
  Gt_traj(const string &file_name):Trajectory(file_name) {}
  Gt_traj(const string &file_name, const Config &c):Trajectory(file_name, c) {}
};

class Es_traj: public Trajectory {
public:
  Es_traj() {}
  Es_traj(const string &file_name, Gt_traj *gt, const Config &c);
  void display_locs();

private:
  void data_output_with_gt();
  void gnuplot_script_generate_with_gt();
  void matlab_script_generate_with_gt();
  int align_timestamp_to_gt();
  Sophus::Sim3d Umeym(map<int, Sophus::SE3d> &mpt,
                   map<int, Sophus::SE3d> &mps,
                   Eigen::Matrix3d& R_, Eigen::Vector3d& t_, double &s_, int &S_,
                   bool use_umeyama,
                   bool consider_rotations,
                   bool align_scale);

  //origin poses align by timestamp, first value is the pose id
  map<int, Sophus::SE3d> poses;
  //matched gt poses
  map<int, Sophus::SE3d> poses_gt;
  //timestamps of poses after aligned which correspoding to poses;
  map<int, double> timestamps_align;
  //es aligned to gt: T^{-1} poses; if whithout gt, the poses_align first trans is transed to 0
  map<int, Sophus::SE3d> poses_align;

  Gt_traj *groundtruth;

  //es = T_estimate * gt
  Sophus::Sim3d T_estimate;

  //T between two body frames, estimate and groundtruth
  Sophus::SE3d T_delta;

  Eigen::Matrix3d R_;
  Eigen::Vector3d t_;
  int S_ = 1;
  double s_ = 1;

  //the color of match line rand seed
  int seed = 1;
};

#endif //_TRAJECTORY_H
